#include "usr_canfd.h"
#include "fdcan.h"



extern FDCAN_HandleTypeDef hfdcan1;



// 发送队列
static CanFdTxMessage tx_queue[CANFD_TX_QUEUE_SIZE];
static uint8_t tx_queue_head = 0;
static uint8_t tx_queue_tail = 0;
static uint8_t tx_queue_count = 0;

static uint8_t fdcan_dlc_map[0x10] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64};

// 接收回调函数和配置
static void (*receive_callback)(FDCAN_RxHeaderTypeDef *, uint8_t *) = NULL;


// 将 数据长度 转换为 FDCAN_DLC 枚举值
static uint32_t get_dlc_from_length(uint8_t length)
{
    if (length <= 8) return FDCAN_DLC_BYTES_8;
    if (length <= 12) return FDCAN_DLC_BYTES_12;
    if (length <= 16) return FDCAN_DLC_BYTES_16;
    if (length <= 20) return FDCAN_DLC_BYTES_20;
    if (length <= 24) return FDCAN_DLC_BYTES_24;
    if (length <= 32) return FDCAN_DLC_BYTES_32;
    if (length <= 48) return FDCAN_DLC_BYTES_48;
    return FDCAN_DLC_BYTES_64;
}

// 将 FDCAN_DLC 枚举值转换为 数据长度
uint8_t get_length_from_dlc(uint8_t dlc)
{
    return fdcan_dlc_map[dlc < 16 ? dlc : 15];
}


/**
 * @brief 向发送队列添加 CAN 消息
 * @param config CAN 帧配置信息
 * @param data 要发送的数据指针
 * @return HAL_StatusTypeDef 添加状态
 */
HAL_StatusTypeDef canfd_add_tx_message(const CanFdConfig *config, uint8_t *data)
{
    static uint8_t irq_enable = 0;
    if(tx_queue_count >= CANFD_TX_QUEUE_SIZE)
    {
        return HAL_ERROR;  // 队列已满
    }

    // 复制配置信息
    tx_queue[tx_queue_tail].config = *config;
    // 复制数据
//    for(int i = 0; i < config->data_length; i++)
//    {
        tx_queue[tx_queue_tail].pdata = data;
//    }

    tx_queue_tail = (tx_queue_tail + 1) % CANFD_TX_QUEUE_SIZE;
    tx_queue_count++;

    // 激活发送中断
    if(irq_enable == 0)
    {
        irq_enable = 1;
        HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_TX_FIFO_EMPTY, 0);
    }

    return HAL_OK;
}

/**
 * @brief 发送队列中的下一条消息
 */
void canfd_send_next_message(void)
{
    if(tx_queue_count == 0)
    {
        return;  // 队列为空
    }

    CanFdTxMessage *msg = &tx_queue[tx_queue_head];
    FDCAN_TxHeaderTypeDef TxHeader;

    TxHeader.Identifier = msg->config.can_id;
    TxHeader.IdType = msg->config.is_extended_id ? FDCAN_EXTENDED_ID : FDCAN_STANDARD_ID;
    TxHeader.TxFrameType = FDCAN_DATA_FRAME;
    TxHeader.DataLength = get_dlc_from_length(msg->config.data_length);
    TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
    TxHeader.BitRateSwitch = FDCAN_BRS_ON;
    TxHeader.FDFormat = FDCAN_FD_CAN;
    TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
    TxHeader.MessageMarker = 0;

    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, msg->pdata);

    // 检查是否为循环发送
    if(msg->config.frame_type == CANFD_FRAME_PERIOD_TYPE)
    {
        // 重新添加到队列
        if(canfd_add_tx_message(&msg->config, msg->pdata) != HAL_OK)
        {
            // 处理添加失败
        }
    }
    tx_queue_head = (tx_queue_head + 1) % CANFD_TX_QUEUE_SIZE;
    tx_queue_count--;
}

/* 
 * @brief 调用此接口向CANFD的TX FIFO添加发送数据，注意一次性只能添加3条报文，否者FIFO将会溢出
 * @param canid CAN 标识符
 * @param data 要发送的数据指针
 * @param len 数据长度
*/
void canfd_send_message(uint32_t canid, uint8_t *data, uint8_t len)
{
    FDCAN_TxHeaderTypeDef TxHeader;
    TxHeader.Identifier = canid;
    TxHeader.IdType = FDCAN_STANDARD_ID;
    TxHeader.TxFrameType = FDCAN_DATA_FRAME;
    TxHeader.DataLength = get_dlc_from_length(len);
    TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
    TxHeader.BitRateSwitch = FDCAN_BRS_ON;
    TxHeader.FDFormat = FDCAN_FD_CAN;
    TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
    TxHeader.MessageMarker = 0;

    HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, data);
}

/**
 * @brief 注册 FDCAN 接收消息回调函数
 * @param config CAN 帧配置信息
 * @param callback 回调函数指针
 */
void canfd_register_receive_callback(void (*callback)(FDCAN_RxHeaderTypeDef *, uint8_t *)) 
{
    receive_callback = callback;
}

// 接收中断回调函数
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
    if(hfdcan == &hfdcan1 && (RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET)
    {
        FDCAN_RxHeaderTypeDef RxHeader;
        static uint8_t FdcanRxData[CANFD_MAX_DATA_LEN];

        if(HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &RxHeader, FdcanRxData) == HAL_OK)
        {
            if(receive_callback != NULL)
            {
                receive_callback(&RxHeader, FdcanRxData);
            }
        }
    }
}

// 发送中断回调函数
void HAL_FDCAN_TxFifoEmptyCallback(FDCAN_HandleTypeDef *hfdcan)
{
    if(hfdcan == &hfdcan1)
    {
//        canfd_send_next_message();
    }
}

/**
 * @brief 配置 FDCAN 接收过滤器
 * @param filter_config 过滤器配置信息
 * @return HAL_StatusTypeDef 配置状态
 */
HAL_StatusTypeDef canfd_configure_filters(const CanFdFilterConfig *filter_config)
{
    if(filter_config->count > CANFD_MAX_FILTERS)
    {
        return HAL_ERROR;  // 过滤器数量超过最大限制
    }
    FDCAN_FilterTypeDef sFilterConfig;
    sFilterConfig.FilterType = FDCAN_FILTER_MASK;
    sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
    sFilterConfig.IdType = filter_config->is_extended_id ? FDCAN_EXTENDED_ID : FDCAN_STANDARD_ID;
    sFilterConfig.FilterID2 = 0x7FF;
    for(uint8_t i = 0; i < filter_config->count; i++)
    {
        sFilterConfig.FilterIndex = i;
        sFilterConfig.FilterID1 = filter_config->ids[i];
        if(HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK)
        {
            return HAL_ERROR;
        }
    }
    if(HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK)
    {
        Error_Handler();
    }
    return HAL_OK;
}











